/**
 * @file   remote.c
 * @author Justin Tanner
 * @date   Mon Sep 15 20:44:50 2008
 * 
 * @brief  Radio demonstration code
 *         This is the sending side of the radio demonstration
 *         Sends a test string to the receiver
 * 
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "radio/radio.h"
#include "uart/uart.h"
#include "sonar/sonar.h"
#include "servo/servo.h"
#include "motor/motor.h"

#define RECEIVE_ADDRESS 0x7777
#define SEND_ADDRESS    0x2345

volatile uint8_t radio_buf[PAYLOAD_BYTES];
uint8_t send_buffer[PAYLOAD_BYTES];

int main(int argc, char**argv)
{
    DISABLE_INTERRUPTS();
    
    CLOCK_8MHZ();

    DDRD |= _BV(PORTD5);
    
    radio_init(SEND_ADDRESS, TRANSMIT_MODE);
	pwm_motors_init();
	init_servo();
	sonar_init();
    
    ENABLE_INTERRUPTS();

	//for sonar
	uint8_t distance = 0;
	uint8_t i = 0;

    for(;;)
    {

		//move into sonar function?
        PORTD ^= _BV(PORTD5);
        
		//fill first 14 of the buffer with 0's 
		for(i=0; i<13; i++)
		{
			send_buffer[i]= '0';
		}

		trigger_sonar(); 

		_delay_ms(46);

		
		distance = read_distance();
		send_buffer[13]= distance;
		//itoa(read_distance(), &send_buffer[13], 10);

/*
		//fill last 13 of the buffer with 0's 
		for(i=14; i<27; i++)
		{
			send_buffer[i]= 'x';
		}
*/

        /* send a full payload (28 bytes) to the receiver */
        radio_send(RECEIVE_ADDRESS, (uint8_t*)send_buffer);
        
        _delay_ms(100);
    }
    
    return 0;
}
